#ifndef __STATE_CONTROL_H
#define __STATE_CONTROL_H

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "stabilizer.h"
#include "config_param.h"
#include "sensors_types.h"
#include "commander.h"
#include "pid.h"




enum PidIndex
{
    RATE_ROLL = 0,
    RATE_PITCH,
    RATE_YAW,
    ANGLE_ROLL,
    ANGLE_PITCH,
    ANGLE_YAW,
    VELOCITY_Z,
    POSHOLD_Z,
    VELOCITY_XY,
    POSHOLD_XY,
    PID_NUM
};



static void all_PID_init(void);
void state_control_init(void);
void state_control(const sensorData_t *SensorData, const state_t *State, setpoint_t *Setpoint, control_t *Control, const u32 tick);
void state_control_reset_YAW_holding(void);
void state_control_set_velocityZ_PID_integration(float integ);



#endif

